#include <cstdio>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/header.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"

class SimulateLidarNode : public rclcpp::Node
{
public:
    SimulateLidarNode() : Node("lidar_node")
    {
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/lidar_points", 10);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&SimulateLidarNode::publishPointCloud, this));
    }

private:
    void publishPointCloud()
    {
        auto header = std_msgs::msg::Header();
        header.stamp = this->now();
        header.frame_id = "base_link"; // Change the frame_id as per your setup

        sensor_msgs::msg::PointCloud2 pointcloud_msg;
        pointcloud_msg.header = header;
        pointcloud_msg.height = 1;
        pointcloud_msg.width = 1000; // Number of points in the point cloud
        pointcloud_msg.is_dense = true;

        sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg);
        modifier.setPointCloud2FieldsByString(1, "xyz");
        sensor_msgs::PointCloud2Iterator<float> iter_x(pointcloud_msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(pointcloud_msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(pointcloud_msg, "z");

        for (size_t i = 0; i < 1000; ++i)
        {
            *iter_x = static_cast<float>(rand()) / RAND_MAX;
            *iter_y = static_cast<float>(rand()) / RAND_MAX;
            *iter_z = static_cast<float>(rand()) / RAND_MAX;
            ++iter_x;
            ++iter_y;
            ++iter_z;
        }

        publisher_->publish(pointcloud_msg);
        RCLCPP_INFO(this->get_logger(), "PointCloud published");
    }

    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto simulate_lidar_node = std::make_shared<SimulateLidarNode>();
  rclcpp::spin(simulate_lidar_node);
  rclcpp::shutdown();

  return 0;
}
